/***********************************************************
 * File Name    : algorithm.c
 * Description  : 心率计算
 * Author       : 
 * Create Date  : 
 * Last Modified: 
 * Version      : 
 * Note         : 
 ***********************************************************/
#include "algorithm.h"
#include <stdint.h>
#include <math.h>

static uint8_t flagUpOrDown         = 0;                    //0代表下降，1代表上升
static uint8_t flagPeaksOrValley    = 0;                    //0代表上一个是波谷，1代表上一个是波峰

static float   MatrixPeaks[PEAKS_VALLEY_MAX]        = {0};   //记录波峰对应的acc数据，滤波处理后的数据
static uint8_t MatrixPeaksIndex[PEAKS_VALLEY_MAX]   = {0};   //记录波峰对应的acc数据索引
static uint8_t Peaks_cnt                            = 0;
static float   MatrixValley[PEAKS_VALLEY_MAX]       = {0};   //记录波谷对应的acc数据，滤波处理后的数据
static uint8_t MatrixValleyIndex[PEAKS_VALLEY_MAX]  = {0};   //记录波谷对应的acc数据索引
static uint8_t Valley_cnt                           = 0;

static float g_acc_x[ACC_DATA_LEN_MAX] = {0};
static float g_acc_y[ACC_DATA_LEN_MAX] = {0};
static float g_acc_z[ACC_DATA_LEN_MAX] = {0};

static float aq3[ACC_DATA_LEN_MAX]      = {0};
static float y_filted[ACC_DATA_LEN_MAX] = {0};

static float GaussTemp[9] = {0};

//当前存储ACC数据总长度
static uint8_t g_u8acc_total_length = 0;

uint16_t g_step_total = 0;    //步数

float f_abs(float a)
{
    if (a >= 0)
    {
        return a;
    }
    else
    {
        return -a;
    }
}

double d_abs(double a)
{
    if (a >= 0)
    {
        return a;
    }
    else
    {
        return -a;
    }
}

void Gauss_para_init(void)
{
    //高斯滤波参数
    for (uint8_t i = 0; i < 9; i++)
    {
        GaussTemp[i] = exp(-(i+1-5)*(i+1-5)/(2*0.49))/(0.7*sqrt(2*M_PI));
    }
}

void Acc_data_Gauss_filter(void)
{
    uint8_t i = 0;

    uint8_t r = 5;
    
    //计算aq3
    for (i = 0; i < g_u8acc_total_length; i++)
    {
        aq3[i] = sqrt((g_acc_x[i] * g_acc_x[i]) + (g_acc_y[i] * g_acc_y[i]) + (g_acc_z[i] * g_acc_z[i]));
    }

    //高斯滤波处理
    for (i = 0; i < g_u8acc_total_length; i++)
    {
        y_filted[i] = aq3[i];
    }

    for (i = r; i <= (g_u8acc_total_length - r); i++)
    {
        y_filted[i] = 0;
        
        for (uint8_t j = 0; j < (2 * r - 1); j++)
        {
            y_filted[i] += aq3[i - r + 1 + j] * GaussTemp[j];
        }
    }
}

void Acc_data_Peaks_Valley_cal(void)
{
    Peaks_cnt = 0;
    Valley_cnt = 0;

    //根据前两个数据设置波形方向
    if (y_filted[1] >= y_filted[0])
    {
        flagUpOrDown = 1;
    }
    else
    {
        flagUpOrDown = 0;
    }

    //分析计算波形和波谷
    for (uint8_t i = 2; i < g_u8acc_total_length; i++)
    {
        //波形由上升转为下降
        if ((1 == flagUpOrDown) && (y_filted[i] < y_filted[i-1]))
        {
            flagUpOrDown = 0;
            
            if (Valley_cnt)
            {
                if ((y_filted[i-1] - MatrixValley[Valley_cnt-1]) < thd)
                {
                    continue;
                }
                else
                {
                    if (Peaks_cnt)
                    {
                        //两次波峰间隔要满足阈值要求，约200ms - 2s内
                        if(((i-1) - MatrixPeaksIndex[Peaks_cnt - 1] < INDEX_BETWEEN_MIN) && ((i-1) - MatrixPeaksIndex[Peaks_cnt - 1] > INDEX_BETWEEN_MAX))
                        {
                            continue;
                        }
                    }

                    //如果上一个是波峰，要特殊处理，是波谷就没有问题
                    if (1 == flagPeaksOrValley)
                    {
                        if(y_filted[i-1] > MatrixPeaks[Peaks_cnt-1])
                        {
                            //使用当前波峰替换上一个
                            MatrixPeaks[Peaks_cnt-1]        = 0;
                            MatrixPeaksIndex[Peaks_cnt-1]   = 0;
                            
                            Peaks_cnt--;
                        }
                        else
                        {
                            continue;
                        }
                    }
                    
                    //记录新的波峰
                    MatrixPeaks[Peaks_cnt]      = y_filted[i-1];
                    MatrixPeaksIndex[Peaks_cnt] = i - 1;
                    Peaks_cnt++;
                    
                    flagPeaksOrValley =1;
                                    
                }
            }
            //不可能两个波峰之间没有波谷, 但要考虑第一个最大值或者最小值，第一个波峰
            else
            {
                if (0 == Peaks_cnt)
                {
                    //第一个波峰
                    if (y_filted[i-1] > thd)
                    {
                        MatrixPeaks[0]      = y_filted[i-1];
                        MatrixPeaksIndex[0] = i - 1;
                        Peaks_cnt++;

                        flagPeaksOrValley   = 1;
                    }
                }
                else
                {
                    //需要更新使用当前的波峰为第一个波峰
                    if (y_filted[i-1] > MatrixPeaks[0])
                    {
                        MatrixPeaks[0]      = y_filted[i-1];
                        MatrixPeaksIndex[0] = i - 1;

                        flagPeaksOrValley   = 1;
                    }
                }
            }
        }
        //波形由下降转为上升
        else if ((0 == flagUpOrDown) && (y_filted[i] > y_filted[i-1]))
        {
            flagUpOrDown = 1;
            
            if (Peaks_cnt)
            {
                //没有达到阈值，不处理。
                if (f_abs(y_filted[i-1] - MatrixPeaks[Peaks_cnt-1]) < thd)
                {
                    continue;
                }
                else
                {
                    if (Valley_cnt)
                    {
                        if (((i-1) - MatrixValleyIndex[Valley_cnt - 1] < INDEX_BETWEEN_MIN) && ((i-1) - MatrixValleyIndex[Valley_cnt - 1] > INDEX_BETWEEN_MAX))
                        {
                            continue;
                        }
                    }

                    //如果上一个是波谷，要处理，是波峰就没有问题
                    if(flagPeaksOrValley == 0)
                    {
                        if (y_filted[i-1] < MatrixValley[Valley_cnt-1])
                        {
                            //使用当前波谷替换上一个
                            MatrixValley[Valley_cnt-1]      = 0;
                            MatrixValleyIndex[Valley_cnt-1] = 0;
                            
                            Valley_cnt--;
                        }
                        else
                        {
                            continue;
                        }
                    }

                    //记录新的波谷
                    MatrixValley[Valley_cnt]        = y_filted[i-1];
                    MatrixValleyIndex[Valley_cnt]   = i-1;
                    Valley_cnt++;
                    
                    flagPeaksOrValley =0;
                }
            }
            //第一次先出现波谷之前没有出现波峰，不处理
            else
            {
            }       
        }
    }

    if (0 == Valley_cnt)
    {
        Peaks_cnt = 0;
    }
}

static void data_handle_1hz(void)
{
    //处理本周期ACC数据，计算步数
    //高斯滤波处理
    Acc_data_Gauss_filter();
    
    //计算波峰/波谷
    Acc_data_Peaks_Valley_cal();
    
    //本周期新增步数
    uint8_t stepnum_second = 0;
    
    //获取中间组ACC开始索引位置及结束索引位置
    #if 0
    uint8_t startindex1 = g_u8acc_sec_group_index;
    uint8_t endindex1   = g_u8acc_sec_group_index + g_u8acc_sec_group_length;
    #endif
    uint8_t startindex1 = FR_ACC;
    uint8_t endindex1   = FR_ACC + FR_ACC;
    
    uint8_t height_start_index = 0;
    uint8_t height_end_index   = 0;
    double  height_second      = 0; 
    uint8_t step_interval[5] = {0};
    uint8_t step_interval_cnt= 0;
    // 根据有效峰值点统计每秒步幅步频信息
    for (uint8_t ii = 0; ii < Peaks_cnt; ii++)
    {
        //计算高度变化
        //每次脚掌落地时（ACC峰值点）假设垂直方向速度为0，垂直高度置0
        double v0       = 0;
        double height   = 0;
    
        //计算高度，从第一组的最后一个波峰开始，到第二组的最后一个波峰结束
        if (ii + 1 < Peaks_cnt)
        {
            //确定计算高度的开始index
            if((MatrixPeaksIndex[ii] < startindex1) && (MatrixPeaksIndex[ii + 1] >= startindex1))
            {
                height_start_index  = MatrixPeaksIndex[ii];
                height_end_index    = MatrixPeaksIndex[ii + 1];
                step_interval[step_interval_cnt++] = height_end_index - height_start_index;//每步时间间隔
            }
            else if ((MatrixPeaksIndex[ii] >= startindex1) && (MatrixPeaksIndex[ii + 1] < endindex1))
            {
                height_start_index  = MatrixPeaksIndex[ii];
                height_end_index    = MatrixPeaksIndex[ii + 1];
                step_interval[step_interval_cnt++] = height_end_index - height_start_index;//每步时间间隔
            }
        }

        //只检测中间一组数据的波峰
        if ((MatrixPeaksIndex[ii] >= startindex1) && (MatrixPeaksIndex[ii] < endindex1))
        {
            stepnum_second++;//每秒步数累计
        }
        //第三组的数据不处理了
        else if (MatrixPeaksIndex[ii] >= endindex1)
        {
            break;
        }
        
    }
    g_step_total +=  stepnum_second; //更新每秒步数
}


void data_collect_Acc(float xxx, float yyy, float zzz)
{
    if (g_u8acc_total_length < ACC_DATA_LEN_MAX)
    {
        g_acc_x[g_u8acc_total_length] = (ONE_G*xxx)/SSVT_A;
        g_acc_y[g_u8acc_total_length] = (ONE_G*yyy)/SSVT_A;
        g_acc_z[g_u8acc_total_length] = (ONE_G*zzz)/SSVT_A;
        g_u8acc_total_length++;
    }
    
    //有3组数据，调用算法计算
    if (ACC_DATA_LEN_MAX == g_u8acc_total_length)
    {
        data_handle_1hz();

        //已经满3组数据，移除掉最早的一组数据
        for (uint8_t i = FR_ACC; i < ACC_DATA_LEN_MAX; i++)
        {
            g_acc_x[i - FR_ACC] = g_acc_x[i];
            g_acc_y[i - FR_ACC] = g_acc_y[i];
            g_acc_z[i - FR_ACC] = g_acc_z[i];
        }
        
        //更新第二组信息
        g_u8acc_total_length = ACC_DATA_LEN_MAX - FR_ACC;
    }
}

